﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Aldebaran.Proxies;
using System.Collections;

namespace grounding.nao.sensors
{
    public class JointPositionSensor : ISensor
    {
        private MemoryProxy memoryProxy;
        private string name;

        public string Name { get { return name; } }
        public string Type { get { return "joint_position"; } }

        /// <summary>
        /// Assumes proxies are connected to robot.
        /// </summary>
        /// <param name="memoryProxy">established MemoryProxy</param>
        public JointPositionSensor(MemoryProxy memoryProxy, string name)
        {
            this.memoryProxy = memoryProxy;
            this.name = name;
        }

        /// <summary>
        /// Polls for the current motor positions.
        /// </summary>
        /// <returns>A JointPositionSensorFrame of the current motor positions.</returns>
        public ISensorFrame GetSensorFrame() { return GetSensorFrame(x => x); }
        /// <summary>
        /// Polls for the current motor positions.
        /// </summary>
        /// <returns>A JointPositionSensorFrame of the current motor positions.</returns>
        public ISensorFrame GetSensorFrame(Func<IEnumerable<double>, IEnumerable<double>> transformer)
        {
            // Prepare list of keys.
            ArrayList keys = new ArrayList();
            keys.Add("Device/SubDeviceList/HeadPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/HeadYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnklePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnkleRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHand/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipYawPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LKneePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LWristYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnklePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnkleRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHand/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RKneePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RWristYaw/Position/Sensor/Value");

            // Fetch the data.
            ArrayList rawValues = (ArrayList)memoryProxy.getListData(keys);

            // Convert to an array of doubles.
            double[] values = new double[rawValues.Count];
            for (int i = 0; i < rawValues.Count; i++)
                values[i] = (float)rawValues[i];

            return new JointPositionSensorFrame(this, transformer(values).ToArray());
        }
    }
}
